      SUBROUTINE CNTREG(A,NA,B,NB,H,NH,Q,NQ,R,NR,Z,W,LAMBDA,S,F,NF,P,NP
     1,T,IOP,IDENT,DUMMY)
C 
C   PURPOSE:
C      Solve the time-invariant continuous-time linear optimal output
C      regulator problem with noise-free measurements.
C 
C   REFERENCES:
C      Kwakernaak, Huibert; and Sivan Raphael: Linear Optimal Control
C        Systems. John Wiley & Sons, Inc., c. 1972.
C      Vaughan, David R.: A Negative Exponential Solution for the Matrix
C        Riccati Equation. IEEE Trans. Autom. Control, vol. AC-14, no.
C        1, Feb. 1969, pp. 72-75.
C 
C   Subroutines employed by CNTREG: ADD, DGECO, DGESL, DPOCO, DPOSL,
C      EIGEN, EQUATE, EXPSER, JUXTR, LNCNT, MAXEL, MULT, NULL, PRNT,
C      SCALE, SUBT, TRANP
C   Subroutines employing CNTREG: ASYREG
C 
      IMPLICIT REAL*8 (A-H,O-Z)
      DIMENSION A(1),B(1),H(1),Q(1),R(1),Z(1),W(1),LAMBDA(1),S(1),F(1),P
     1(1),T(1),DUMMY(1)
      DIMENSION NA(2),NB(2),NH(2),NQ(2),NR(2),NF(2),NP(2),IOP(3),NDUM1(2
     1),NDUM2(2)
      LOGICAL IDENT
      REAL*8 LAMBDA
      COMMON/CONV/SUMCV,MAXSUM,RICTCV,SERCV
C 
      IF( IOP(1). EQ. 0 ) GO TO 65
      CALL LNCNT(5)
      IF( IOP(3) .EQ. 0 ) WRITE(6,25)
   25 FORMAT(//' PROGRAM TO SOLVE THE TIME-INVARIANT FINITE-DURATION CON
     1TINUOUS OPTIMAL'/' REGULATOR PROBLEM WITH NOISE-FREE MEASUREMENTS'
     2)
      IF( IOP(3) .NE. 0 ) WRITE(6,30)
   30 FORMAT(//' PROGRAM TO SOLVE THE TIME-INVARIANT INFINITE-DURATION C
     1ONTINUOUS OPTIMAL'/' REGULATOR PROBLEM  WITH NOISE-FREE MEASUREMEN
     2TS')
      CALL PRNT(A,NA,' A  ',1)
      CALL PRNT(B,NB,' B  ',1)
      CALL PRNT(Q,NQ,' Q  ',1)
      IF( .NOT. IDENT ) GO TO 45
      CALL LNCNT(3)
      WRITE(6,35)
   35 FORMAT(/' H IS AN IDENTITY MATRIX'/)
      GO TO 55
C 
   45 CONTINUE
      CALL PRNT(H,NH,' H  ',1)
      CALL MULT(Q,NQ,H,NH,DUMMY,NH)
      N1= NH(1)*NH(2)+1
      CALL TRANP(H,NH,DUMMY(N1),NDUM1)
      CALL MULT(DUMMY(N1),NDUM1,DUMMY,NH,Q,NQ)
      CALL LNCNT(3)
      WRITE(6,50)
   50 FORMAT(//' MATRIX (H TRANSPOSE)QH')
      CALL PRNT(Q,NQ,0,3)
   55 CONTINUE
      CALL PRNT(R,NR,' R  ',1)
C 
      IF( IOP(3) .NE. 0 ) GO TO 65
      CALL LNCNT(4)
      WRITE(6,60)
   60 FORMAT(//' WEIGHTING ON TERMINAL VALUE OF STATE VECTOR'/)
      CALL PRNT(P,NP,' P  ',1)
C 
   65 CONTINUE
      CALL EQUATE(R,NR,DUMMY,NR)
      N = NA(1)**2
      N1 = NB(1)*NB(2)+1
      CALL TRANP(B,NB,DUMMY(N1),NDUM1)
      N2 = N1 + N
      L = NR(1)
C 
C   * * * CALL TO MATHLIB FUNCTIONS * * *
      CALL DPOCO(DUMMY,L,L,RCOND,DUMMY(N2),IERR)
      IF( IERR .EQ. 0 ) GO TO 100
      CALL LNCNT(4)
      WRITE(6,75)
   75 FORMAT(//' IN CNTREG, THE SUBROUTINE  DPOCO HAS FOUND THE MATRIX
     1 R NOT POSITIVE DEFINITE'/)
      RETURN
C 
  100 CONTINUE
      NT = N1
      DO 150 M1 = 1,NB(1)
         CALL DPOSL(DUMMY,L,L,DUMMY(NT))
         NT = NT + L
  150 CONTINUE
      CALL EQUATE(DUMMY(N1),NDUM1,DUMMY,NDUM1)
      CALL MULT(B,NB,DUMMY(N1),NDUM1,DUMMY(N2),NA)
      CALL SCALE(DUMMY(N2),NA,DUMMY(N1),NA,-1.0D0)
      N3 = N2 + N
      IF( IDENT .OR. (IOP(1) .NE. 0) ) GO TO 200
      CALL MULT(Q,NQ,H,NH,DUMMY(N2),NH)
      CALL TRANP(H,NH,DUMMY(N3),NDUM1)
      CALL MULT(DUMMY(N3),NDUM1,DUMMY(N2),NH,Q,NQ)
C 
  200 CONTINUE
      CALL SCALE(Q,NQ,Q,NQ,-1.0D0)
      CALL JUXTR(A,NA,Q,NQ,Z,NDUM1)
      CALL TRANP(A,NA,DUMMY(N2),NA)
      CALL SCALE( DUMMY(N2),NA,DUMMY(N2),NA,-1.0D0)
      L = 2*N + 1
      CALL JUXTR(DUMMY(N1),NA,DUMMY(N2),NA,Z(L),NDUM1)
      CALL SCALE(Q,NQ,Q,NQ,-1.0D0)
      NDUM2(1) = 2*NA(1)
      NDUM2(2) = NDUM2(1)
      IF( IOP(1) .NE. 0 )  CALL PRNT(Z,NDUM2,' Z  ',1)
      CALL EQUATE(Z,NDUM2,DUMMY(N1),NDUM2)
      M = 4*N
      N2 = M + N1
      L = 2*NA(1)
      N3 = N2 + L
      N4 = N3 + L
      ISV = L
      ILV = 0
      CALL EIGEN(L,L,DUMMY(N1),DUMMY(N2),DUMMY(N3),ISV,ILV,W,DUMMY(N4),I
     1ERR)
      IF( IERR .EQ. 0 ) GO TO 300
      CALL LNCNT(4)
      IF( IERR .GT. 0 ) GO TO 250
      WRITE(6,225) IERR
  225 FORMAT(//' IN CNTREG, EIGEN FAILED TO COMPUTE THE ' ,I6 ,' EIGENV
     1ECTOR OF Z '/)
      RETURN
  250 CONTINUE
      WRITE(6,275) IERR
  275 FORMAT(//' IN CNTREG, THE ',I6 ,' EIGENVALUE OF Z HAS NOT BEEN FO
     1UND AFTER 30 ITERATIONS IN EIGEN'/)
      RETURN
C 
  300 CONTINUE
      IF( IOP(1) .EQ. 0 ) GO TO 400
      CALL LNCNT(3)
      WRITE(6,325)
  325 FORMAT(//' EIGENVALUES OF Z')
      NDUM1(1) = L
      NDUM1(2) = 2
      CALL PRNT(DUMMY(N2),NDUM1,0,3)
      CALL LNCNT(3)
      WRITE(6,350)
  350 FORMAT(//' CORRESPONDING EIGENVECTORS')
      CALL PRNT(W,NDUM2,0,3)
C 
  400 CONTINUE
      CALL EQUATE(W,NDUM2,DUMMY(N1),NDUM2)
      J1 = 1
      J2 = 1
      M = 2*N
      NDUM1(1) = L
      NDUM1(2) = 1
      K4 = N4
C 
      I=1
  415 CONTINUE
      IF( I .GT. L )  GO TO 515
      K1 = N2+I-1
      K2 = N1+(I-1)*L
      K3 = N3+I-1
      IF(DUMMY(K1) .GT. 0.0 ) GO TO 425
      J = (J1-1)*L+M+1
      J1 = J1+1
      IF(DUMMY(K3).NE. 0.0) J1=J1+1
      GO TO 450
  425 CONTINUE
      DUMMY(K4)=I
      K4 = K4+1
      J = (J2-1)*L+1
      J2 = J2+1
      IF( DUMMY(K3) .NE. 0.0 )  J2 = J2 + 1
  450 CONTINUE
      CALL EQUATE(DUMMY(K2),NDUM1,W(J),NDUM1)
      IF(DUMMY(K3) .EQ. 0.0) GO TO 500
      I = I+1
      K2 = K2+L
      J = J+L
      CALL EQUATE(DUMMY(K2),NDUM1,W(J),NDUM1)
  500 CONTINUE
      I=I+1
      GO TO 415
  515 CONTINUE
C 
      CALL NULL(LAMBDA,NA)
      K0 = -1
      J = -NA(1)
      NAX = NA(1)
      I=1
  520 CONTINUE
      IF( I .GT. NAX )  GO TO 530
      J = NAX + J + 1
      K0 = K0 + 1
      K1 = N4 + K0
      K2 = DUMMY(K1)
      K = N2+K2-1
      LAMBDA(J) = DUMMY(K)
      K3 = N3+K2-1
      IF( DUMMY(K3) .EQ. 0.0 ) GO TO 525
      K4 = J+1
      LAMBDA(K4) = -DUMMY(K3)
      K4 = K4+NAX
      LAMBDA(K4) = DUMMY(K)
      K4 = K4-1
      LAMBDA(K4) = DUMMY(K3)
      K5 = M + (I-1)*L + 1
      K6 = K5 + L
      CALL EQUATE(W(K5),NDUM1,DUMMY(N1),NDUM1)
      CALL EQUATE(W(K6),NDUM1,W(K5),NDUM1)
      CALL EQUATE(DUMMY(N1),NDUM1,W(K6),NDUM1)
      I = I+1
      J = NAX + J +1
  525 CONTINUE
      I=I+1
      GO TO 520
  530 CONTINUE
C 
      IF( IOP(1) .EQ. 0 ) GO TO 700
      CALL LNCNT(3)
      WRITE(6,535)
  535 FORMAT(//' REORDERED EIGENVECTORS')
      CALL PRNT(W,NDUM2,0,3)
      CALL LNCNT(4)
      WRITE(6,545)
  545 FORMAT(//' LAMBDA MATRIX OF EIGENVALUES OF Z WITH POSITIVE REAL PA
     1RTS'/)
      CALL PRNT(LAMBDA,NA,0,3)
C 
      CALL MULT(Z,NDUM2,W,NDUM2,DUMMY(N1),NDUM2)
      L = NDUM2(1)
      M = L**2
      N2 = N1+M
      CALL EQUATE(W,NDUM2,DUMMY(N2),NDUM2)
      N3 = N2+M
      N4 = N3+L
C 
C   * * * CALL TO MATHLIB FUNCTIONS * * *
      CALL DGECO(DUMMY(N2),L,L,DUMMY(N3),RCOND,DUMMY(N4))
      IF ((1.0 + RCOND) .NE. 1.0) GO TO 600
      CALL LNCNT(4)
      WRITE(6,550) RCOND
  550 FORMAT(//' IN CNTREG, DGECO HAS FOUND THE REORDERED MATRIX W TO B
     1E SINGULAR, RCOND = ',D16.8,/)
  600 CONTINUE
      NT = N1
      DO 650 M1 = 1,L
         CALL DGESL(DUMMY(N2),L,L,DUMMY(N3),DUMMY(NT),0)
         NT = NT + L
  650 CONTINUE
      CALL PRNT(DUMMY(N1),NDUM2,'WIZW',1)
C 
  700 CONTINUE
      NDUM1(1) = 2*NA(1)
      NDUM1(2) = NA(1)
      N2 = 2*N + N1
      CALL TRANP(W,NDUM1,DUMMY(N2),NDUM2)
      NW11 = N1
      NDUM1(1) = NA(1)
      CALL TRANP(DUMMY(N2),NDUM1,DUMMY(NW11),NDUM1)
      L = N2+N
      NW21 = NW11+N
      CALL TRANP(DUMMY(L),NDUM1,DUMMY(NW21),NDUM1)
      L = 2*N+1
      NDUM1(1)=2*NA(1)
      N3 = N2 + 2*N
      CALL TRANP(W(L),NDUM1,DUMMY(N3),NDUM2)
      NDUM1(1) = NA(1)
      NW12 = NW21+N
      CALL TRANP(DUMMY(N3),NDUM1,DUMMY(NW12),NDUM1)
      L = N3 + N
      NW22 = NW12 + N
      CALL TRANP(DUMMY(L),NDUM1,DUMMY(NW22),NDUM1)
C 
      IF( IOP(1) .EQ. 0 ) GO TO 800
      CALL PRNT(DUMMY(NW11),NA,'W11 ',1)
      CALL PRNT(DUMMY(NW21),NA,'W21 ',1)
      CALL PRNT(DUMMY(NW12),NA,'W12 ',1)
      CALL PRNT(DUMMY(NW22),NA,'W22 ',1)
C 
  800 CONTINUE
      IF( IOP(3) .NE. 0 ) GO TO 900
      N2 = N1+4*N
      CALL MULT(P,NP,DUMMY(NW12),NA,S,NA)
      CALL MULT(P,NP,DUMMY(NW11),NA,DUMMY(N2),NA)
      CALL SUBT(S,NA,DUMMY(NW22),NA,S,NA)
      CALL SUBT(DUMMY(NW21),NA,DUMMY(N2),NA,DUMMY(N2),NA)
      N3 = N2+N
      L = NA(1)
      N4 = N3+NA(1)
C 
C   * * * CALL TO MATHLIB FUNCTIONS * * *
      CALL DGECO(DUMMY(N2),L,L,DUMMY(N3),RCOND,DUMMY(N4))
      IF ((1.0 + RCOND) .NE. 1.0) GO TO 850
      CALL LNCNT(4)
      WRITE(6,825) RCOND
  825 FORMAT(//' IN CNTREG, DGECO HAS FOUND THE MATRIX  W21 - P1XW11 TO
     1 BE SINGULAR, RCOND = ',D16.8,/)
      RETURN
C 
  850 CONTINUE
      NT = 1
      DO 860 M1 = 1,L
         CALL DGESL(DUMMY(N2),L,L,DUMMY(N3),S(NT),0)
         NT = NT + L
  860 CONTINUE
      IF( IOP(1) .EQ. 0 ) GO TO 1000
      CALL PRNT(S,NA,' S  ',1)
      NDUM1(1) = NR(1)
      NDUM1(2) = NA(1)
      CALL LNCNT(3)
      WRITE(6,875)
  875 FORMAT(//' MATRIX (R INVERSE)X(B TRANSPOSE)')
      CALL PRNT(DUMMY,NDUM1,0,3)
      GO TO 1000
C 
  900 CONTINUE
      N2 = N1+4*N
      CALL TRANP(DUMMY(NW12),NA,DUMMY(N2),NA)
      CALL TRANP(DUMMY(NW22),NA,P,NP)
      N3 = N2+N
      L = NA(1)
      N4 = N3 + NA(1)
C 
C   * * * CALL TO MATHLIB FUNCTIONS * * *
      CALL DGECO(DUMMY(N2),L,L,DUMMY(N3),RCOND,DUMMY(N4))
      IF ((1.0 + RCOND) .NE. 1.0) GO TO 950
      CALL LNCNT(4)
      WRITE(6,925) RCOND
  925 FORMAT(//' IN CNTREG, DGECO HAS FOUND THE MATRIX W12 TO BE SINGUL
     1AR, RCOND = ',D16.8,/)
      RETURN
  950 CONTINUE
      NT = 1
      DO 975 M1 = 1,L
         CALL DGESL(DUMMY(N2),L,L,DUMMY(N3),P(NT),0)
         NT = NT + L
  975 CONTINUE
      NDUM1(1) = NR(1)
      NDUM1(2) = NA(1)
      CALL MULT(DUMMY,NDUM1,P,NP,F,NF)
      IF( IOP(1) .EQ. 0 ) RETURN
      CALL PRNT(P,NP,' P  ',1)
      CALL PRNT(F,NF,' F  ',1)
      RETURN
C 
 1000 CONTINUE
      NMAX = T(1)/T(2)
      I = NMAX
      CALL EQUATE(LAMBDA,NA,DUMMY(N2),NA)
      TT = -T(2)
      N4 = N3+N
      N5 = N4+N
      N6 = N5+N
      N7 = N6+NA(1)
      KSS = 0
      NDUM1(1) = NR(1)
      NDUM1(2) = NA(1)
      CALL EXPSER(DUMMY(N2),NA,DUMMY(N3),NA,TT,KSS,DUMMY(N4))
      CALL EQUATE(DUMMY(N3),NA,DUMMY(N2),NA)
      IF( IOP(1) .EQ. 0 )  GO TO 1075
      CALL LNCNT(3)
      WRITE(6,1050) T(2)
 1050 FORMAT(//' EXP(-LAMBDA X ',D16.8 ,')')
      CALL PRNT(DUMMY(N2),NA,0,3)
 1075 CONTINUE
      IF( NMAX .LE. 0 )  RETURN
      CALL EQUATE(S,NA,DUMMY(N3),NA)
 1100 CONTINUE
      TIME = I*T(2)
      IF( I .NE. NMAX )  CALL EQUATE(DUMMY(N5),NA,P,NP)
      CALL MULT(DUMMY(N3),NA,DUMMY(N2),NA,DUMMY(N4),NA)
      CALL MULT(DUMMY(N2),NA,DUMMY(N4),NA,DUMMY(N3),NA)
      CALL MULT(DUMMY(NW11),NA,DUMMY(N3),NA,DUMMY(N4),NA)
      CALL ADD(DUMMY(NW12),NA,DUMMY(N4),NA,DUMMY(N4),NA)
      CALL TRANP(DUMMY(N4),NA,DUMMY(N5),NA)
      CALL EQUATE(DUMMY(N5),NA,DUMMY(N4),NA)
      CALL MULT(DUMMY(NW21),NA,DUMMY(N3),NA,DUMMY(N5),NA)
      CALL ADD(DUMMY(NW22),NA,DUMMY(N5),NA,DUMMY(N5),NA)
      CALL TRANP(DUMMY(N5),NA,DUMMY(N6),NA)
      CALL EQUATE(DUMMY(N6),NA,DUMMY(N5),NA)
      L = NA(1)
C 
C   * * * CALL TO MATHLIB FUNCTIONS * * *
      CALL DGECO(DUMMY(N4),L,L,DUMMY(N6),RCOND,DUMMY(N7))
      IF ((1.0 + RCOND) .NE. 1.0) GO TO 1200
      CALL LNCNT(3)
      WRITE(6,1150) TIME,RCOND
 1150 FORMAT(//' IN CNTREG AT TIME ',D16.8,'  P CANNOT BE COMPUTED DUE
     1 TO MATRIX SINGULARITY IN DGECO, RCOND = ',D16.8)
      RETURN
C 
 1200 CONTINUE
      NT = N5
      DO 1220 M1 = 1,L
         CALL DGESL(DUMMY(N4),L,L,DUMMY(N6),DUMMY(NT),0)
         NT = NT + L
 1220 CONTINUE
      CALL MAXEL(P,NP,ANORM1)
      CALL SUBT(DUMMY(N5),NA,P,NP,DUMMY(N4),NA)
      CALL MAXEL(DUMMY(N4),NA,ANORM2)
      IF( ANORM1 .NE. 0.0 ) GO TO 1225
      GO TO 1300
C 
 1225 CONTINUE
      IF(ANORM1 .GT. 1.0 ) GO TO 1250
      IF( ANORM2/ANORM1 .LT. RICTCV ) KSS=1
      GO TO 1300
 1250 CONTINUE
      IF( ANORM2 .LT. RICTCV ) KSS=1
C 
 1300 CONTINUE
      CALL MULT(DUMMY,NDUM1,P,NP,F,NF)
      IF( IOP(2) .EQ. 0 ) GO TO 1400
      CALL LNCNT(5)
      WRITE(6,1350) TIME
 1350 FORMAT(///' TIME = ',D16.8/)
      CALL PRNT(P,NP,' P  ',1)
      IF( I .NE. NMAX )  CALL PRNT(F,NF,' F  ',1)
C 
 1400 CONTINUE
      IF( KSS .EQ. 1 ) GO TO 1500
      I = I-1
      IF( I .GE. 0 ) GO TO 1100
      GO TO 1600
 1500 CONTINUE
      CALL LNCNT(4)
      WRITE(6,1550)
 1550 FORMAT(//' STEADY-STATE SOLUTION HAS BEEN REACHED IN CNTREG'/)
C 
 1600 CONTINUE
      IF( IOP(2) .NE. 0 ) RETURN
      IF( IOP(1) .EQ. 0 ) RETURN
      CALL LNCNT(5)
      WRITE(6,1350) TIME
      CALL PRNT(P,NP,' P  ',1)
      CALL PRNT(F,NF,' F  ',1)
C 
      RETURN
      END
